#include <stdlib.h>
#include <string>

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include "tf/transform_datatypes.h"

#define MY_PI 3.1415926

using namespace std;

double current_pos[3] = {0.0,0.0,0.0};
double begin_pos[3] = {0.0,0.0,0.0};
double s = 0.2;
double last_yaw = 0.;
double total_rotate = 0.;

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    current_pos[0] = msg->pose.pose.position.x;
    current_pos[1] = msg->pose.pose.position.y;
    double roll, pitch, yaw;
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);
    tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
    current_pos[2] = yaw;
}

double get_rotate_from_start()
{
    double delta = 0.0;
    if(current_pos[2] - last_yaw > MY_PI)
    {
        delta = 2*MY_PI - (current_pos[2] - last_yaw);
    }
    else if(current_pos[2] - last_yaw > 0)
    {
        delta = current_pos[2] - last_yaw;
    }
    else if(current_pos[2] - last_yaw < -MY_PI)
    {
        delta = -2*MY_PI - (current_pos[2] - last_yaw);
    }
    else
    {
        delta = current_pos[2] - last_yaw;
    }
    last_yaw = current_pos[2];
    total_rotate += delta;
//    ROS_INFO("total rotate:%f",total_rotate);
    return total_rotate;
}

bool run_forward()
{
    if(current_pos[0] - begin_pos[0] > 1.0)
        return true;
    else
        return false;    
}

bool run_backward()
{
    if(current_pos[0] - begin_pos[0] < -1.0)
        return true;
    else
        return false;    
}

bool turn_left()
{
    if(get_rotate_from_start()>2*MY_PI)
        return true;
    else
        return false;    
}

bool turn_right()
{
    if(get_rotate_from_start()<-2*MY_PI)
        return true;
    else
        return false;    
}

bool run_round()
{   
    if(get_rotate_from_start() > 2*MY_PI)
        return true;
    else
        return false;
}

int main(int argc, char *argv[])
{

    sleep(10);
    ros::init(argc,argv,"smartgo_control");
    ros::NodeHandle nh("~");
    ros::Publisher cmd_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",2);
    ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>("/odom",2,odomCallback);
    string type = nh.param<string>("control_type", "forward");
    
    ros::Rate r(20);
    while(ros::ok())
    {
        geometry_msgs::Twist twist;
        if(type == "forward")
        {
            if(!run_forward())
            {
                twist.linear.x = 0.5;
                twist.linear.y = 0.;
                twist.linear.z = 0.;
                twist.angular.x = 0.0;
                twist.angular.y = 0.0;
                twist.angular.z = 0.0;
//                ROS_INFO("forward");
            }
            else 
                return 0;
        }
        else if (type == "backward")
        {
            if(!run_backward())
            {
                twist.linear.x = -0.5;
                twist.linear.y = 0.;
                twist.linear.z = 0.;
                twist.angular.x = 0.0;
                twist.angular.y = 0.0;
                twist.angular.z = 0.0;
            }
            else 
                return 0;
        }
        else if (type == "left")
        {
            if(!turn_left())
            {
                twist.linear.x = 0.0;
                twist.linear.y = 0.;
                twist.linear.z = 0.;
                twist.angular.x = 0.0;
                twist.angular.y = 0.0;
                twist.angular.z = 0.5;
            }
            else 
                return 0;
        }
        else if (type == "right")
        {
            if(!turn_right())
            {
                twist.linear.x = 0.0;
                twist.linear.y = 0.;
                twist.linear.z = 0.;
                twist.angular.x = 0.0;
                twist.angular.y = 0.0;
                twist.angular.z = -0.5;
            }
            else 
                return 0;
        }
        else if (type == "round")
        {
                if(!run_round())
            {
                twist.linear.x = 0.3;
                twist.linear.y = 0.;
                twist.linear.z = 0.;
                twist.angular.x = 0.0;
                twist.angular.y = 0.0;
                twist.angular.z = 0.8;
            }
            else 
                return 0;
        }

        cmd_pub.publish(twist);
        ros::spinOnce();
        r.sleep();
    }
}
